
#include <cmath>
#include <sstream>
#include "Matrix4.h"

#include "Math.h"

using namespace Iride;

const Matrix4 Matrix4::ZERO = Matrix4(
	0.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 0.0f);

const Matrix4 Matrix4::IDENTITY = Matrix4(
	1.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 1.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 1.0f);


Matrix4::Matrix4()
{
}

Matrix4::Matrix4(
Real m00, Real m01, Real m02, Real m03,
Real m10, Real m11, Real m12, Real m13,
Real m20, Real m21, Real m22, Real m23,
Real m30, Real m31, Real m32, Real m33)
{
	m[0][0] = m00;
    m[0][1] = m01;
    m[0][2] = m02;
    m[0][3] = m03;
    m[1][0] = m10;
    m[1][1] = m11;
    m[1][2] = m12;
    m[1][3] = m13;
    m[2][0] = m20;
    m[2][1] = m21;
    m[2][2] = m22;
    m[2][3] = m23;
    m[3][0] = m30;
    m[3][1] = m31;
    m[3][2] = m32;
    m[3][3] = m33;
}



Real* Matrix4::operator[] (const size_t row)
{
    return m[row];
}

const Real* Matrix4::operator[] (const size_t row ) const
{
    return m[row];
}

Matrix4 Matrix4::operator- (void) const
{
	Matrix4 result;
	result.m[0][0] = - m[0][0];
	result.m[0][1] = - m[0][1];
	result.m[0][2] = - m[0][2];
	result.m[0][3] = - m[0][3];
	result.m[1][0] = - m[1][0];
	result.m[1][1] = - m[1][1];
	result.m[1][2] = - m[1][2];
	result.m[1][3] = - m[1][3];
	result.m[2][0] = - m[2][0];
	result.m[2][1] = - m[2][1];
	result.m[2][2] = - m[2][2];
	result.m[2][3] = - m[2][3];
	result.m[3][0] = - m[3][0];
	result.m[3][1] = - m[3][1];
	result.m[3][2] = - m[3][2];
	result.m[3][3] = - m[3][3];
	return result;
}

Matrix4 Matrix4::operator+ (const Matrix4& other) const
{
	Matrix4 result;
	result.m[0][0] = m[0][0] + other.m[0][0];
	result.m[0][1] = m[0][1] + other.m[0][1];
	result.m[0][2] = m[0][2] + other.m[0][2];
	result.m[0][3] = m[0][3] + other.m[0][3];
	result.m[1][0] = m[1][0] + other.m[1][0];
	result.m[1][1] = m[1][1] + other.m[1][1];
	result.m[1][2] = m[1][2] + other.m[1][2];
	result.m[1][3] = m[1][3] + other.m[1][3];
	result.m[2][0] = m[2][0] + other.m[2][0];
	result.m[2][1] = m[2][1] + other.m[2][1];
	result.m[2][2] = m[2][2] + other.m[2][2];
	result.m[2][3] = m[2][3] + other.m[2][3];
	result.m[3][0] = m[3][0] + other.m[3][0];
	result.m[3][1] = m[3][1] + other.m[3][1];
	result.m[3][2] = m[3][2] + other.m[3][2];
	result.m[3][3] = m[3][3] + other.m[3][3];

	return result;
}

Matrix4 Matrix4::operator- (const Matrix4& other) const
{
	Matrix4 result;
	result.m[0][0] = m[0][0] - other.m[0][0];
	result.m[0][1] = m[0][1] - other.m[0][1];
	result.m[0][2] = m[0][2] - other.m[0][2];
	result.m[0][3] = m[0][3] - other.m[0][3];
	result.m[1][0] = m[1][0] - other.m[1][0];
	result.m[1][1] = m[1][1] - other.m[1][1];
	result.m[1][2] = m[1][2] - other.m[1][2];
	result.m[1][3] = m[1][3] - other.m[1][3];
	result.m[2][0] = m[2][0] - other.m[2][0];
	result.m[2][1] = m[2][1] - other.m[2][1];
	result.m[2][2] = m[2][2] - other.m[2][2];
	result.m[2][3] = m[2][3] - other.m[2][3];
	result.m[3][0] = m[3][0] - other.m[3][0];
	result.m[3][1] = m[3][1] - other.m[3][1];
	result.m[3][2] = m[3][2] - other.m[3][2];
	result.m[3][3] = m[3][3] - other.m[3][3];

	return result;
}

Matrix4 Matrix4::operator* (const Matrix4& other) const
{
	Matrix4 result;
	result.m[0][0] = m[0][0] * other.m[0][0]  +  m[0][1] * other.m[1][0]  + m[0][2] * other.m[2][0]  +  m[0][3] * other.m[3][0];
	result.m[0][1] = m[0][0] * other.m[0][1]  +  m[0][1] * other.m[1][1]  + m[0][2] * other.m[2][1]  +  m[0][3] * other.m[3][1];
	result.m[0][2] = m[0][0] * other.m[0][2]  +  m[0][1] * other.m[1][2]  + m[0][2] * other.m[2][2]  +  m[0][3] * other.m[3][2];
	result.m[0][3] = m[0][0] * other.m[0][3]  +  m[0][1] * other.m[1][3]  + m[0][2] * other.m[2][3]  +  m[0][3] * other.m[3][3];

	result.m[1][0] = m[1][0] * other.m[0][0]  +  m[1][1] * other.m[1][0]  + m[1][2] * other.m[2][0]  +  m[1][3] * other.m[3][0];
	result.m[1][1] = m[1][0] * other.m[0][1]  +  m[1][1] * other.m[1][1]  + m[1][2] * other.m[2][1]  +  m[1][3] * other.m[3][1];
	result.m[1][2] = m[1][0] * other.m[0][2]  +  m[1][1] * other.m[1][2]  + m[1][2] * other.m[2][2]  +  m[1][3] * other.m[3][2];
	result.m[1][3] = m[1][0] * other.m[0][3]  +  m[1][1] * other.m[1][3]  + m[1][2] * other.m[2][3]  +  m[1][3] * other.m[3][3];

	result.m[2][0] = m[2][0] * other.m[0][0]  +  m[2][1] * other.m[1][0]  + m[2][2] * other.m[2][0]  +  m[2][3] * other.m[3][0];
	result.m[2][1] = m[2][0] * other.m[0][1]  +  m[2][1] * other.m[1][1]  + m[2][2] * other.m[2][1]  +  m[2][3] * other.m[3][1];
	result.m[2][2] = m[2][0] * other.m[0][2]  +  m[2][1] * other.m[1][2]  + m[2][2] * other.m[2][2]  +  m[2][3] * other.m[3][2];
	result.m[2][3] = m[2][0] * other.m[0][3]  +  m[2][1] * other.m[1][3]  + m[2][2] * other.m[2][3]  +  m[2][3] * other.m[3][3];

	result.m[3][0] = m[3][0] * other.m[0][0]  +  m[3][1] * other.m[1][0]  + m[3][2] * other.m[2][0]  +  m[3][3] * other.m[3][0];
	result.m[3][1] = m[3][0] * other.m[0][1]  +  m[3][1] * other.m[1][1]  + m[3][2] * other.m[2][1]  +  m[3][3] * other.m[3][1];
	result.m[3][2] = m[3][0] * other.m[0][2]  +  m[3][1] * other.m[1][2]  + m[3][2] * other.m[2][2]  +  m[3][3] * other.m[3][2];
	result.m[3][3] = m[3][0] * other.m[0][3]  +  m[3][1] * other.m[1][3]  + m[3][2] * other.m[2][3]  +  m[3][3] * other.m[3][3];

	return result;
}

Matrix4 Matrix4::operator* (const Real& coeff) const
{
	Matrix4 result;
	result.m[0][0] = coeff * m[0][0];
	result.m[0][1] = coeff * m[0][1];
	result.m[0][2] = coeff * m[0][2];
	result.m[0][3] = coeff * m[0][3];
	result.m[1][0] = coeff * m[1][0];
	result.m[1][1] = coeff * m[1][1];
	result.m[1][2] = coeff * m[1][2];
	result.m[1][3] = coeff * m[1][3];
	result.m[2][0] = coeff * m[2][0];
	result.m[2][1] = coeff * m[2][1];
	result.m[2][2] = coeff * m[2][2];
	result.m[2][3] = coeff * m[2][3];
	result.m[3][0] = coeff * m[3][0];
	result.m[3][1] = coeff * m[3][1];
	result.m[3][2] = coeff * m[3][2];
	result.m[3][3] = coeff * m[3][3];

	return result;
}

Matrix4 Matrix4::operator/ (const Real& coeff) const
{
	Matrix4 result;
	result.m[0][0] = m[0][0] / coeff;
	result.m[0][1] = m[0][1] / coeff;
	result.m[0][2] = m[0][2] / coeff;
	result.m[0][3] = m[0][3] / coeff;
	result.m[1][0] = m[1][0] / coeff;
	result.m[1][1] = m[1][1] / coeff;
	result.m[1][2] = m[1][2] / coeff;
	result.m[1][3] = m[1][3] / coeff;
	result.m[2][0] = m[2][0] / coeff;
	result.m[2][1] = m[2][1] / coeff;
	result.m[2][2] = m[2][2] / coeff;
	result.m[2][3] = m[2][3] / coeff;
	result.m[3][0] = m[3][0] / coeff;
	result.m[3][1] = m[3][1] / coeff;
	result.m[3][2] = m[3][2] / coeff;
	result.m[3][3] = m[3][3] / coeff;

	return result;
}

Vector4 Matrix4::operator* (const Vector4& vec) const
{
	Vector4 result;
	result[0] = m[0][0] * vec[0] +  m[0][1] * vec[1] + m[0][2] * vec[2] +  m[0][3] * vec[3];
	result[1] = m[1][0] * vec[0] +  m[1][1] * vec[1] + m[1][2] * vec[2] +  m[1][3] * vec[3];
	result[2] = m[2][0] * vec[0] +  m[2][1] * vec[1] + m[2][2] * vec[2] +  m[2][3] * vec[3];
	result[3] = m[3][0] * vec[0] +  m[3][1] * vec[1] + m[3][2] * vec[2] +  m[3][3] * vec[3];

	return result;
}

Vector3 Matrix4::operator* (const Vector3& vec) const
{
	Vector3 result;
	result[0] = m[0][0] * vec[0] +  m[0][1] * vec[1] + m[0][2] * vec[2] +  m[0][3];
	result[1] = m[1][0] * vec[0] +  m[1][1] * vec[1] + m[1][2] * vec[2] +  m[1][3];
	result[2] = m[2][0] * vec[0] +  m[2][1] * vec[1] + m[2][2] * vec[2] +  m[2][3];

	return result;
}

Matrix4& Matrix4::operator+= (const Matrix4& other)
{
  *this = *this + other;
  return *this;
}

Matrix4& Matrix4::operator-= (const Matrix4& other)
{
  *this = *this - other;
  return *this;
}
std::string Matrix4::ToString() const
{
	std::stringstream out;
	out << m[0][0] << " " << m[0][1] << " " << m[0][2] << " " << m[0][3] << std::endl
	<< m[1][0] << " " << m[1][1] << " " << m[1][2] << " " << m[1][3] << std::endl
	<< m[2][0] << " " << m[2][1] << " " << m[2][2] << " " << m[2][3] << std::endl
	<< m[3][0] << " " << m[3][1] << " " << m[3][2] << " " << m[3][3] << std::endl;
	return out.str();
}

Matrix4 Matrix4::transpose(void) const
{
	return Matrix4(
					m[0][0], m[1][0], m[2][0], m[3][0],
					m[0][1], m[1][1], m[2][1], m[3][1],
					m[0][2], m[1][2], m[2][2], m[3][2],
					m[0][3], m[1][3], m[2][3], m[3][3]
					);

}

void Matrix4::MakeRotateX(Real angle)
{
	Real sin = Math::Sin(angle);
	Real cos = Math::Cos(angle);

	m[0][0] = 1; m[0][1] = 0;   m[0][2] = 0;     m[0][3] = 0;
	m[1][0] = 0; m[1][1] = cos; m[1][2] = -sin;  m[1][3] = 0;
    m[2][0] = 0; m[2][1] = sin; m[2][2] = cos;   m[2][3] = 0;
    m[3][0] = 0; m[3][1] = 0;   m[3][2] = 0;     m[3][3] = 1;
}

void Matrix4::MakeRotateY(Real angle)
{
	Real sin = Math::Sin(angle);
	Real cos = Math::Cos(angle);

	m[0][0] = cos;  m[0][1] = 0; m[0][2] = sin; m[0][3] = 0;
	m[1][0] = 0;    m[1][1] = 1; m[1][2] = 0;   m[1][3] = 0;
    m[2][0] = -sin; m[2][1] = 0; m[2][2] = cos; m[2][3] = 0;
    m[3][0] = 0;    m[3][1] = 0; m[3][2] = 0;   m[3][3] = 1;
}

void Matrix4::MakeRotateZ(Real angle)
{
	Real sin = Math::Sin(angle);
	Real cos = Math::Cos(angle);

	m[0][0] = cos; m[0][1] = -sin; m[0][2] = 0; m[0][3] = 0;
	m[1][0] = sin; m[1][1] = cos;  m[1][2] = 0; m[1][3] = 0;
    m[2][0] = 0;   m[2][1] = 0;    m[2][2] = 0; m[2][3] = 0;
    m[3][0] = 0;   m[3][1] = 0;    m[3][2] = 0; m[3][3] = 1;
}

void Matrix4::MakeTranslate(const Vector4 &trans)
{
	m[0][0] = 1;  m[0][1] = 0; m[0][2] = 0; m[0][3] = trans[0];
	m[1][0] = 0;  m[1][1] = 1; m[1][2] = 0; m[1][3] = trans[1];
    m[2][0] = 0;  m[2][1] = 0; m[2][2] = 1; m[2][3] = trans[2];
    m[3][0] = 0;  m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
}
